//
#include "common.h"
//HANDLER FOR RANDOM INTERRUPTS
//http://www.nongnu.org/avr-libc/user-manual/group__avr__interrupts.html
//External Pin, Power-on Reset, Brown-out Reset, Watchdog Reset, and JTAG AVR Rese
ISR(BADISR_vect)
{
   	//PCIF1 15:8
	//printf("bad ISR\n");
	if(EIFR & (1<<PCIF1))
	{
		printf("Pin Change interrupt\n");

	}	else if( TIFR0 & _BV(OCF0A)){
	   //printf("timer0 interrupt\n");
	  
	}
}

ISR(PCINT1_vect)
{

	if(flagcheck(flags, FLAG_RFDATA_AVAIL)){ //packet current being processed
	  flagset(error, ERR_RFPROC);	
	} else {
	  flagset(flags, FLAG_RFDATA_AVAIL);
		//packet_rf_process();
	}
} 

void packet_rf_handler(void)
{
	static u08 count = 0;
	u08 temp;
	//mask off checksum
	temp = rf_packet[0] & 0xF0;
	
	if((count - temp) != 0)
	{
		flagset(error, ERR_RFPACKET);
	}
	
	count = temp;
	
	if(rf_packet[0] == RFPACKET_ADCSTREAMH)
	{
		//buzzerPOST();
		//flagset(error, ERR_RFPACKET);
	} else if(rf_packet[0] == RFPACKET_ADCSTREAML)
	{
		flagset(error, ERR_RFPACKET);
		LED_PORT ^= BV(LED_PIN);
		//graph_sample = rf_packet[2];
		chan0 = rf_packet[1]; //ac amp
		chan1 = rf_packet[2]; //notch
		chan2 = rf_packet[3]; //battery voltage
			/*
		rf_packet[1] = a2dConvert8bit(2); //AC amp
		rf_packet[2] = a2dConvert8bit(1); //notch
		rf_packet[3] = a2dConvert8bit(0); //battery
		*/
	}
}

void error_handler(void)
{
	if (error) 
	{
		#ifndef FPGA
			printf("ERROR: %x", error);
		#endif
		/*
		if(error_check(ERR_RFPROC)) {
		
		}
		if(error_check(ERR_RFPROC)) {
		
		}
		*/
		if (flagcheck(error, ERR_RFPACKET))
		{
		}
		static int temp = 0;
	//	temp++;
		temp = error;
		
		if(temp > 999) temp = 0;
		fpga_packet[1] = temp / 100;
		fpga_packet[2] = (temp % 100) / 10;
		fpga_packet[3] = (temp % 10);
		packet_send_FPGA(PACKET_SAO2);
	} 
	
	error = 0;
}

void flag_handler(void)
{

	if(flagcheck(flags, FLAG_RFDATA_AVAIL)){ //RF data available
		packet_recv_rf();
		//buzzerPOST();
	} 
	if(flagcheck(flags, FLAG_RFDATA_RDY)){ //RF data ready for transmission
		packet_send_rf();
	}
	
	if(flagcheck(flags, FLAG_HEART_BEAT)){ //buzz
		
	}
	
	if(flagcheck(flags, FLAG_SEND_GRAPH)){
		u08 temp;
		//temp = a2dConvert8bit(0);
		temp = 255-BEEP_CHANNEL;
		
		avg_dc = avg(temp);
		bpm = heartrate(avg_dc);
		if(noise)
		{
			temp = 0;
			
			fpga_packet[1] = 0;
			fpga_packet[2] = 0;
			fpga_packet[3] = 0;
			packet_send_FPGA(PACKET_BPM);
		} else {

			if(beep) {	
				fpga_packet[1] = bpm / 100;
				fpga_packet[2] = (bpm % 100) / 10;
				fpga_packet[3] = (bpm % 10);
				packet_send_FPGA(PACKET_BPM);
			} 	
		}		
	//	static u08 i=0;++i;
		if(!noise){
				fpga_packet[1] = avg_dc;//255-chan0;
				fpga_packet[2] = 255-chan1;
				fpga_packet[3] = 255-chan2;
			} else { 
				fpga_packet[1] = 0;
				fpga_packet[2] = 0;
				fpga_packet[3] = 0;
			}

		//fpga_packet[2] = i;
			/*
		rf_packet[1] = a2dConvert8bit(2); //AC amp
		rf_packet[2] = a2dConvert8bit(1); //notch
		rf_packet[3] = a2dConvert8bit(0); //battery
		*/
		packet_send_FPGA(PACKET_GRAPH); //send graph packet
		
		rf_packet[1] = 0;
		rf_packet[2] = 0;
		rf_packet[3] = 0;
	}
	
	flags = 0;
}

void packet_send_rf(void)
{
	static u08 count = 0;
	if(rf24g_mode == RX_MODE) {
		flagset(error, ERR_RFMODE);
	} else if(rf24g_mode == TX_MODE) {
	    //append count nibble
		rf_packet[0] |= count++ << 4;
		transmit_data();
	
		#ifdef UART_RFECHO
			printf("RFP SENT: %02X %02X %02X %02X\n", rf_packet[0], rf_packet[1], rf_packet[2], rf_packet[3]);	
		#endif
	}
	
}
void packet_recv_rf(void)
{
	
	if(rf24g_mode == RX_MODE) {

		receive_data();
		#ifdef UART_RFECHO
			printf("RFP RECVD: %02X %02X %02X %02X\n", rf_packet[0], rf_packet[1], rf_packet[2], rf_packet[3]);
		#endif
	} else if(rf24g_mode == TX_MODE) {
		flagset(error, ERR_RFMODE);
	}
}

void init_ADC(void)
{
//code from
//http://www.engr.sjsu.edu/bjfurman/courses/ME106/ME106pdf/A2D_ATmega128.pdf
	printf("Initializing ADC...\r\n");
	a2dInit();

	DDRF = 0x00;	// configure a2d port as input

	PORTF = ADC_PULLUPS;

	// set the a2d prescaler (clock division ratio)
	// - a lower prescale setting will make the a2d converter go faster
	// - a higher setting will make it go slower but the measurements
	//   will be more accurate
	// - other allowed prescale values can be found in a2d.h
	a2dSetPrescaler(ADC_PRESCALE_DIV32);

	// set the a2d reference
	// - the reference is the voltage against which a2d measurements are made
	// - other allowed reference values can be found in a2d.h
	a2dSetReference(ADC_REFERENCE_AVCC);

	// use a2dConvert8bit(channel#) to get an 8bit a2d reading
	// use a2dConvert10bit(channel#) to get a 10bit a2d reading
}

void streamADC(void)
{
	u08 i;
		//tx(0xAA);
		//tx(0xBB);

	rf_packet[0] = RFPACKET_ADCSTREAMH;	
	for(i = 1; i <= 3; i++)
		rf_packet[i] = a2dConvert8bit(i-1);
	packet_send_rf();
	
	rf_packet[0] = RFPACKET_ADCSTREAML;	
	for(i = 4; i <= 7; i++)
		rf_packet[i-3] = a2dConvert8bit(i-1);
		
	packet_send_rf();
	
	//flag RF packet ready
}

u08 heartrate (unsigned char c)
{
       static u08 avg_bpm = 0;
       static u08 i = 0;
	   static u08 pulsecount = 0;
       static u08 count = 1;
	   static u16 nopulse = 0;
	   static u08 beepcount = 1;
       static u08 buffer[N_AVG+1] = {0};
       static u08 flag = 0;

       if (c > BPM_SIGNAL_CROSSOVER)
       {
		   if (!flag)
		   {	
		   		   nopulse = 0;
				   flag = 1;
				   beep = 1;
				   if (beepcount > 30)
					   pulsecount++;
				   if (!noise && beepcount > 30 && pulsecount == 3){
					pulsecount=0;
					sbi(BUZZER_PORT, BUZZER_PIN); 
				   }
				   if (i > N_AVG)
						   i = 0;
				   buffer[i] = ADC_SAMPLERATE*60/count;
				   avg_bpm += (buffer[i] - buffer [(i+1)%(N_AVG+1)])/N_AVG;
				   if(count < 21){
						noise = 1;
				   } else noise = 0;
				   
				   beepcount = 1;
				   count = 1;
				   i++;
		   } else {
		   		    nopulse++;
				    beepcount++;
					beep = 0;
				   if (count > 8){
					cbi(BUZZER_PORT, BUZZER_PIN); 
					}
					if(count == 0) count = 1;
					else count++;
					
					if(count > 21)
					 noise = 0;
		
			}
       } else {
	   			nopulse++;
				beep = 0;
				flag = 0;
				beepcount++;
				if(count > 21)
					 noise = 0;		   
				
				if (count > 8){
					cbi(BUZZER_PORT, BUZZER_PIN); 
				}
				
				if(count == 0) count = 1;
				else count++;
       }	 
	   if (nopulse > 500){
	   	beep = 1;
	   	return 0;
		}
	   else
        return avg_bpm;
}

void packet_send_FPGA(u08 packet_type)
{
	//extern u08 fpga_packet[FPGA_PACKET_LEN];//defined in common.h
	fpga_packet[0] = packet_type;
	#ifdef FPGA	
		u08 i;
		u08 checksum = 0;
		
		for(i = 0; i < FPGA_PACKET_LEN-1;i++)
		{		
			  uart_tx(fpga_packet[i]);
			  checksum += fpga_packet[i];
		}
		fpga_packet[FPGA_PACKET_LEN-1] = checksum;
		uart_tx(fpga_packet[FPGA_PACKET_LEN-1]);
		
	#else
		#warning "REMINDER: FPGA PACKET DISABLED!!!!"
		#warning "REMINDER: FPGA PACKET DISABLED!!!!"
		#warning "REMINDER: FPGA PACKET DISABLED!!!!"
	#endif
	
}

unsigned char rms(unsigned char c1)
{
       static int avg = 0;
       static unsigned char i = 0;
       static int buffer[rms_N+1] = {0};
       unsigned char rms_c;

       if (i > rms_N)
               i = 0;
       buffer[i] = c1*c1;
       avg += buffer[i] - buffer [(i+1)%(rms_N+1)];
       rms_c = sqrt(avg/rms_N);
       i++;
       return rms_c;
}

unsigned char avg(unsigned char c1)
{
       static unsigned int sum = 0;
       static unsigned char avg = 0;
       static unsigned char i = 0;
       static unsigned char buffer[dc_N+1] = {0};

       if (i > dc_N)
               i = 0;
       buffer[i] = c1;
       sum += (buffer[i] - buffer [(i+1)%(dc_N+1)]);
       avg = sum/dc_N;
       i++;
       return avg;
}
